#include <cstdio>
#include <cassert>
#include <vector>
#include <iostream>

using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <semaphore.h>

using namespace cv;

#include <cstring>

#include "protocol.h"
#include "timestamp.h"
#include "transferdaemon.h"
#include "cameradaemon.h"
#include "ptzcontrol.h"
#include "targettrack.h"

#define PRINT_SIZE(x) printf("sizeof("#x") = %u\n",sizeof(x))
#define TIME()  TimeStamp::now()->print()

//components
CameraDaemon g_camera;
TransferDaemon g_transfer;
PtzControl g_ptz;
TargetTrack g_track;

//for check req
TimeStamp g_last_req_ts;

//for image_request
Mat g_img;
TimeStamp g_img_ts;
vector<unsigned char> g_img_data;

bool g_send_cancel = true;
pthread_t g_send_thread;




requestHandler* g_image_request_handler;
requestHandler* g_ptz_request_handler;
requestHandler* g_camera_request_handler;
requestHandler* g_track_request_handler;


void gRequestHandler(normal_request_t request, struct sockaddr_in addr, socklen_t addr_len){
    if(requestInfo(&request)->magic != REQUEST_MAGIC)//magic code check
        return;
    //pandaboard doesn't have battery, can't keep correct time
//    if (g_last_req_ts > requestInfo(&request)->timestamp){//timestamp out of date
//        return;
//    }else{
//        g_last_req_ts = requestInfo(&request)->timestamp;
//    }
    switch(requestInfo(&request)->type){
    case IMAGE_REQUEST:
        (*g_image_request_handler)(request, addr, addr_len);
        break;
    case PTZ_REQUEST:
        (*g_ptz_request_handler)(request, addr, addr_len);
        break;
//    case CAMERA_REQUEST:
//        (*g_camera_request_handler)(request, addr, addr_len);
//        break;
    case TRACK_REQUEST:
        (*g_track_request_handler)(request, addr, addr_len);
        printf("track response time %d ms\n",TimeStamp::now()->diff(requestInfo(&request)->timestamp));
        break;
    default:
        break;
    }

}

void sendResponseHead(response_head_t& response, struct sockaddr_in addr, socklen_t addr_len){
    response.timestamp = TimeStamp::now()->toInt();
    response.magic = RESPONSE_MAGIC;
    g_transfer.sendData((void*) &response, sizeof(response_head_t), addr, addr_len);
}

void sendResponseData(response_data_t& data, struct sockaddr_in addr, socklen_t addr_len){
    responseInfo(&data)->timestamp = TimeStamp::now()->toInt();
    responseInfo(&data)->magic = RESPONSE_MAGIC;
    g_transfer.sendData((void*) &data, sizeof(response_data_t), addr, addr_len);
}



typedef struct send_routine_para{
    int seq;
    int count;
    struct sockaddr_in addr;
    socklen_t addr_len;
    bool* cancel;
}send_routine_para_t;

send_routine_para_t g_send_para;

void* sendRoutine(void* para){
    send_routine_para_t* send_para = (send_routine_para_t*) para;
    for(int i = send_para->seq;!(*send_para->cancel) && i < send_para->seq + send_para->count;i++){
        response_data_t data;
        int len;
        int remain = g_img_data.size() - i * RESPONSE_DATA_LEN;
        if(remain > 0){
            len = (unsigned int) remain > RESPONSE_DATA_LEN ? RESPONSE_DATA_LEN : remain;
            responseInfo(&data)->type = IMAGE_RESPONSE;
            setResponseDataInfo(&data, IMAGE_TIMESTAMP, g_img_ts.toInt());
            setResponseDataInfo(&data, IMAGE_SEQ, i);
            setResponseDataInfo(&data, IMAGE_SEQ_SIZE, len);
            memcpy(responseDataPtr(&data), &g_img_data[i * RESPONSE_DATA_LEN], len);
            sendResponseData(data, send_para->addr, send_para->addr_len);
        }else{
            break;
        }

    }
    (*send_para->cancel) = true;
    return (void*) 0;
}

void sendTrackResponse(TimeStamp ts, Rect area, Point2f center, struct sockaddr_in addr, socklen_t addr_len){
    response_head_t response;
    response.type = TRACK_RESPONSE;
    setResponseField(&response, TRACK_TIMESTAMP, ts.toInt());
    setResponseField(&response, TRACK_PT_X1, area.x);
    setResponseField(&response, TRACK_PT_Y1, area.y);
    setResponseField(&response, TRACK_PT_X2, area.x + area.width);
    setResponseField(&response, TRACK_PT_Y2, area.y + area.height);
    setResponseField(&response, TRACK_PT_CENTER_X, center.x);
    setResponseField(&response, TRACK_PT_CENTER_Y, center.y);
    sendResponseHead(response, addr, addr_len);
}

sem_t g_send_mutex;
sem_t g_send_splitted_mutex[3];
bool g_sending;
int g_send_seq;
int g_send_count;
struct sockaddr_in g_send_addr;
socklen_t g_send_addr_len;

void* sendDaemon(void* para){
    struct sockaddr_in _addr;
    socklen_t _addr_len;
    int _seq;
    int _count;
    while(true){
        sem_wait(&g_send_mutex);
        _addr = g_send_addr;
        _addr_len = g_send_addr_len;
        _seq = g_send_seq;
        _count = g_send_count;
        g_sending = false;
        for(int i = _seq; i < _seq + _count; i++){
            response_data_t _data;
            int _len;
            int _remain = g_img_data.size() - i * RESPONSE_DATA_LEN;
            if(_remain > 0){
                _len = (unsigned int) _remain > RESPONSE_DATA_LEN ? RESPONSE_DATA_LEN : _remain;
                responseInfo(&_data)->type = IMAGE_RESPONSE;
                setResponseDataInfo(&_data, IMAGE_TIMESTAMP, g_img_ts.toInt());
                setResponseDataInfo(&_data, IMAGE_SEQ, i);
                setResponseDataInfo(&_data, IMAGE_SEQ_SIZE, _len);
                memcpy(responseDataPtr(&_data), &g_img_data[i * RESPONSE_DATA_LEN], _len);
                sendResponseData(_data, _addr, _addr_len);

            }else{
                break;
            }
        }
    }
}





void sendImageSeq(int seq,  struct sockaddr_in addr, socklen_t addr_len, int count, bool* cancel){

//    printf("\n");
//    TimeStamp::now()->print("\ngot request");
    g_send_addr = addr;
    g_send_addr_len = addr_len;
    g_send_seq = seq;
    g_send_count = count;
    g_sending = true;
    sem_post(&g_send_mutex);


}

struct sockaddr_in g_ptz_addr;
socklen_t g_ptz_addr_len;
bool g_ptz_ok = false;
void sendPtzResponse(int x, int y, int z, struct sockaddr_in addr, socklen_t addr_len){
    response_head_t response;
    response.type = PTZ_RESPONSE;
    setResponseField(&response, PTZ_X, x);
    setResponseField(&response, PTZ_Y, y);
    setResponseField(&response, PTZ_Z, z);
    sendResponseHead(response, addr, addr_len);
}
void gPtzRequestHandler(normal_request_t request, struct sockaddr_in addr, socklen_t addr_len){
    assert(requestInfo(&request)->type == PTZ_REQUEST);

    int x = getRequestField(&request, PTZ_X);
    int y = getRequestField(&request, PTZ_Y);
    int z = getRequestField(&request, PTZ_Z);
    g_ptz.set(x, y, z);

    printf("get %d %d %d\n", x, y, z);
    g_ptz.get(x, y, z);
    printf("now %d %d %d\n", x, y, z);

//    x++;
//    y++;
//    z++;
    g_ptz_addr = addr;
    g_ptz_addr_len = addr_len;
    g_ptz_ok = true;
    sendPtzResponse(x, y, z, addr, addr_len);


}

void gTrackRequestHandler(normal_request_t request, struct sockaddr_in addr, socklen_t addr_len){
    assert(requestInfo(&request)->type == TRACK_REQUEST);
    TimeStamp ts = (unsigned int)getRequestField(&request, TRACK_TIMESTAMP);
    int x1 = getRequestField(&request, TRACK_PT_X1);
    int y1 = getRequestField(&request, TRACK_PT_Y1);
    int x2 = getRequestField(&request, TRACK_PT_X2);
    int y2 = getRequestField(&request, TRACK_PT_Y2);

    printf("got track req %d %d %d %d\n", x1, y1, x2, y2);
    g_track.startTarck(ts, x1, y1, x2, y2);
}

void gImageRequestHandler(normal_request_t request, struct sockaddr_in addr, socklen_t addr_len){
    assert(requestInfo(&request)->type == IMAGE_REQUEST);
    unsigned int ts;
    int seq;
    int width;
    int height;
    int count;

    int size;

    ts = (unsigned int)getRequestField(&request, IMAGE_TIMESTAMP);
    seq = getRequestField(&request, IMAGE_SEQ);
    width = getRequestField(&request, IMAGE_WIDTH);
    height = getRequestField(&request, IMAGE_HEIGHT);
    count = getRequestField(&request, IMAGE_TRAN_COUNT);

    if(seq == -1){
        g_camera.setWidth(width);
        g_camera.setHeight(height);
        if(g_track.getTrackImage(g_img, g_img_ts)){
            g_img_data.clear();
            imencode(".jpg", g_img, g_img_data);
            size = g_img_data.size();
            response_head_t response;
            response.type = IMAGE_RESPONSE;
            setResponseField(&response, IMAGE_TIMESTAMP, g_img_ts.toInt());
            setResponseField(&response, IMAGE_SEQ, seq);
            setResponseField(&response, IMAGE_SIZE, size);
            sendResponseHead(response, addr, addr_len);
        }
    }else if(g_img_ts == ts){
        assert(count >= 0);
        if((seq + count) * RESPONSE_DATA_LEN >= g_img_data.size()){
            count = ((g_img_data.size() + RESPONSE_DATA_LEN - 1) / RESPONSE_DATA_LEN - seq);
            count = count >= 0 ? count : 0;
        }

        sendImageSeq(seq, addr, addr_len, count, &g_send_cancel);

    }


}

void testtimestamp(){
    int b;
    unsigned int c;
    for(unsigned int i =1; i<=0xf0000000; i*=2){
        b = (int) i;
        c = (unsigned int)b;
        if(c==i){
            printf("\r%u", i);
        }else{
            printf("\ri = %u c = %u\n");
        }
    }
    printf("\nfinish\n");
}

const char* g_keys =
{
    "{c| camera |-1   | camera number}"
    "{f| file   |false| use file as source}"
    "{np|nopreview| false| without preview}"
    "{1|        |nofile   |source file}"
    "{w|wait    |-1   |go straight to the frame}"
    "{b|begin   | 0    |begin frame}"
    "{s|save    |false| save video}"
};


void sendPtzDaemon(){
    static int count = 0;
    if(!g_ptz_ok)
        return;
    if(count > 30){
        count = 0;
        int x, y, z;
        g_ptz.get(x, y, z);
        sendPtzResponse(x, y, z, g_ptz_addr, g_ptz_addr_len);
    }
    count ++;
}

void mainRoutine(int argc, const char** argv){
    //test struct size
    PRINT_SIZE(request_head_t);
    PRINT_SIZE(response_head_t);
    PRINT_SIZE(response_data_t);

//    testtimestamp();

    TimeStamp* t = new TimeStamp;
    t->print();


    //parse command line
    CommandLineParser parser(argc, argv, g_keys);
    int device = parser.get<int>("camera");
    bool use_file = parser.get<bool>("file");
    string file_name = parser.get<string>("1");
    bool no_preview = parser.get<bool>("nopreview");
    bool save_video = parser.get<bool>("save");

    printf("device = %d\n", device);
    if(use_file){
        printf("filename = %s\n", file_name.c_str());
    }else{
        printf("usefile = false\n");
    }
    if(no_preview){
        printf("nopreview = true\n");
    }else{
        printf("nopreview = false\n") ;
    }
    if(save_video){
        printf("save file = true\n");
    }else{
        printf("save file = false\n");
    }
    fflush(stdout);


    //initial

    g_ptz.initial();

    g_sending = false;
    sem_init(&g_send_mutex, 0, 0);


    pthread_t g_send_daemon;
    pthread_create(&g_send_daemon, NULL, sendDaemon, NULL);


    g_last_req_ts.setNow();
    g_image_request_handler = &gImageRequestHandler;
    g_track_request_handler = &gTrackRequestHandler;
    g_ptz_request_handler = &gPtzRequestHandler;

    // prepare components
    if(use_file){
        g_camera.startCapture(file_name, 30);
    }else{
        g_camera.startCapture(device, 24);

    }

    g_track.setCamera(&g_camera);
    g_transfer.setRequestHandler(&gRequestHandler);
    g_transfer.startAnswer();

    if(save_video){
        g_camera.startRecord();
    }

    // main routine
    if(!no_preview)
        namedWindow("preview", 1);
    Mat img;
    TimeStamp ts;
    for(;;){
        g_track.nextFrame();
        if(g_track.getTrackResult(img)){
            if(!no_preview)
                imshow("preview", img);
        }
        int cost = TimeStamp::now()->diff(ts);
        int interval;
        if( cost < 40 && cost >= 0 ){
           interval = 40 - cost;
        }else{
            interval = 1;
        }
        ts.setNow();
        sendPtzDaemon();
        if(waitKey(interval) > 0)
            break;

    }
    pthread_cancel(g_send_daemon);

    void* a;
    pthread_join(g_send_daemon, &a);
}

void test(){
    g_camera.startCapture(-1, 24);
    TimeStamp ts;
    while(1){
        Mat img;
        g_camera.getImage(img, ts);
        if(!img.empty()){
            imshow("preview", img);
        }
        if(waitKey(30)>0){
            break;
        }
        img.release();
    }
}

void testSerialPort(int argc, const char **argv){
    CSerial ptz;
    char * tagf = "ttyACM";
    unsigned char set_pwm[20];
    memset(set_pwm,'\0',20);
    int pwm;
    if(ptz.OpenSerial(tagf,0,115200,8,1,1)){
        printf("serial port ok!\n");
    }
    if(ptz.StartSerial()){
            cout << "Start receivings the data" << endl;
    }
    while(1){
            cout << "Please input the pwm value:";
            cin >> pwm;
            ptz.SetPwmToStr(2,pwm,set_pwm);
//            printf("%s\n",set_pwm);
            cout << endl;
            ptz.SendData(set_pwm,12);
    //		sleep(100);
        }


}

int main(int argc, const char** argv)
{
//    test();

    mainRoutine(argc, argv);
//    Test::testComponents(argc, argv);
//    Test::testBasic(argc, argv);
//    Test test;
//testSerialPort(argc, argv);
    return 0;
}
